clc;
clear;
close all;

A = [-0.313 56.7 0; -0.0139 -0.426 0; 0 56.7 0];
B = [0.232; 0.0203; 0];
C = [0 0 1];
D = [0];

ss_pitch=ss(A,B,C,D);
x=ones(3,1);
k=4;

mpc=InitMPC(ss_pitch,10,1,0);
mpc=MPCSampling(mpc,k,k,[]);
mpc=MPCbound(mpc,createBounds(ss_pitch,k,[10],[],1),createBounds(ss_pitch,k,[10],[],1),100,1:k);
mpc=addMPCstate(mpc,x);
mpc=addMPCcontrol(mpc,0);
tic
[u,u_pred]=MPC(mpc);
toc
u
u_pred


%% Tp~=Tvz
Tp=10;
Tvz=5;
mpc=MPCSampling(mpc,Tp,Tvz,blockMatrix(ss_pitch,Tvz,[1,2;3,4;5,6;7,8;9,10]));

mpc=MPCbound(mpc,createBounds(ss_pitch,Tvz,[-Inf],[],-1),createBounds(ss_pitch,Tvz,[Inf],[],1),100,1:Tp);
tic
[u,u_pred]=MPC(mpc);
toc
u
u_pred

%% mimo system


G=tf(1,[1,2])*[tf([1,-1],[1,-1.1]),tf(1,[1,-1.1]);tf(90,[1,-1.1]),tf([10,-10],[1,-1.1])];
system=c2d(ss(G),0.1);
Tp=50;
Tvz=50;

mpc=InitMPC(system,10*eye(2),eye(2),zeros(2));
mpc=MPCSampling(mpc,Tp,Tvz,[]);
mpc=MPCbound(mpc,createBounds(system,Tvz,[-100;-100],[],-1),createBounds(system,Tvz,[100;100],[],1),zeros(2,1),1:Tp);
mpc=addMPCstate(mpc,ones(4,1));
mpc=addMPCcontrol(mpc,zeros(2,1));

tic
[u,u_pred]=MPC(mpc);
toc
u

%% mimo system test
G=tf(1,[1,2])*[tf([1,-1],[1,-1.1]),tf(1,[1,-1.1]);tf(90,[1,-1.1]),tf([10,-10],[1,-1.1])];
system=c2d(ss(G),0.01);

for i=1:100;
    i
    Tp=i;
    Tvz=i;
    mpc=InitMPC(system,10*eye(2),eye(2),zeros(2));
    mpc=MPCSampling(mpc,Tp,Tvz,[]);
    mpc=MPCbound(mpc,createBounds(system,Tvz,[-100;-100],[],-1),createBounds(system,Tvz,[100;100],[],1),zeros(2,1),1:Tp);
    mpc=addMPCstate(mpc,ones(4,1));
    mpc=addMPCcontrol(mpc,zeros(2,1));
    tic
    [u,u_pred]=MPC(mpc);
    toc
    u
    u_pred
end

%% mimo system block

G=tf(1,[1,2])*[tf([1,-1],[1,-1.1]),tf(1,[1,-1.1]);tf(90,[1,-1.1]),tf([10,-10],[1,-1.1])];
system=c2d(ss(G),0.01);
Tp=50;
Tvz=10;

mpc=InitMPC(system,10*eye(2),eye(2),zeros(2));
mpc=MPCSampling(mpc,Tp,Tvz,blockMatrix(system,Tvz,[1,5;6,10;11,15;16,20;21,25;26,30;31,35;36,40;41,45;46,50]));
mpc=MPCbound(mpc,createBounds(system,Tvz,[-100;-100],[],-1),createBounds(system,Tvz,[100;100],[],1),zeros(2,1),1:Tp);
mpc=addMPCstate(mpc,ones(4,1));
mpc=addMPCcontrol(mpc,zeros(2,1));

tic
[u,u_pred]=MPC(mpc);
toc
u

%% mimo system block ref

G=tf(1,[1,2])*[tf([1,-1],[1,-1.1]),tf(1,[1,-1.1]);tf(90,[1,-1.1]),tf([10,-10],[1,-1.1])];
system=c2d(ss(G),0.01);
Tp=50;
Tvz=10;

mpc=InitMPC(system,10*eye(2),eye(2),zeros(2));
mpc=MPCSampling(mpc,Tp,Tvz,blockMatrix(system,Tvz,[1,5;6,10;11,15;16,20;21,25;26,30;31,35;36,40;41,45;46,50]));
mpc=MPCbound(mpc,createBounds(system,Tvz,[-100;-100],[],-1),createBounds(system,Tvz,[100;100],[],1),zeros(2,1),1:Tp);
mpc=addMPCstate(mpc,ones(4,1));
mpc=addMPCcontrol(mpc,zeros(2,1));
mpc=addMPCref(mpc,ones(Tp*2,1));

tic
[u,u_pred]=MPC(mpc);
toc
u